package org.nashua.tt151;


import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.SimpleRobot;
import java.io.IOException;
import org.nashua.tt151.Controller.DualAction;
import org.nashua.tt151.Dashboard.ConnectionListener;

public class AutoMove extends SimpleRobot {
    private Jaguar frontLeft = new Jaguar(RobotMap.Jaguar.FRONT_LEFT);
    private Jaguar frontRight = new Jaguar(RobotMap.Jaguar.FRONT_RIGHT);
    private Jaguar backLeft = new Jaguar(RobotMap.Jaguar.BACK_LEFT);
    private Jaguar backRight = new Jaguar(RobotMap.Jaguar.BACK_RIGHT);
    private DualAction controller = new DualAction(1);
    private Dashboard dashboard;
    private Relay led = new Relay(1);
    public void robotInit() {
        led.setDirection(Relay.Direction.kForward);
        led.set(Relay.Value.kOn);
        connectToDashboard();
        if (dashboard!=null) {
            try {
                dashboard.sendRaw("[ECH Hello]");
            } catch (IOException ex) {
                ex.printStackTrace();
            }
            //lol
        }
    }
    public void autonomous() {
        while (dashboard!=null && isEnabled() && isOperatorControl()) {
            try {
                // ALigN HIGhest DIStance 5ft 5.0in
                String values = dashboard.requestInfo("ALN HIG DIS 05 05 00");
            } catch (IOException ex) {
                ex.printStackTrace();
            }
        }
    }
    public void connectToDashboard() {
        dashboard = new Dashboard("10.1.51.5", new ConnectionListener(){
            public void onDataRecieved(String str) {
                if (str.equals("[Disconnect]")) {
                    dashboard = null;
                }
                System.out.println(str);
            }
        });
        System.out.println("Connected(?)");
    }
    public void disabled() {
        if (dashboard==null) {
            connectToDashboard();
        }
    }
    public void operatorControl() {
        while (isEnabled() && isOperatorControl()) {
            
        }
    }
}
